#include <iostream>
#include <memory>
#include <random>
#include <vector>
#include <filesystem>
#include <algorithm>
#include <pcl/visualization/pcl_visualizer.h>

template <typename PointT>
typename pcl::PointCloud<PointT>::Ptr transformPointCloud(const typename pcl::PointCloud<PointT>::Ptr &cloudIn, const Eigen::Matrix4f &transform)
{
    typename pcl::PointCloud<PointT>::Ptr cloudOut(new pcl::PointCloud<PointT>());
    pcl::copyPointCloud(*cloudIn, *cloudOut);
    int cloudSize = cloudIn->size();
#pragma omp parallel for
    for (int i = 0; i < cloudSize; i++)
    {
        cloudOut->points[i].x = transform(0, 0) * cloudIn->points[i].x + transform(0, 1) * cloudIn->points[i].y + transform(0, 2) * cloudIn->points[i].z + transform(0, 3);
        cloudOut->points[i].y = transform(1, 0) * cloudIn->points[i].x + transform(1, 1) * cloudIn->points[i].y + transform(1, 2) * cloudIn->points[i].z + transform(1, 3);
        cloudOut->points[i].z = transform(2, 0) * cloudIn->points[i].x + transform(2, 1) * cloudIn->points[i].y + transform(2, 2) * cloudIn->points[i].z + transform(2, 3);
    }
    return cloudOut;
}
template <typename PointT>
typename pcl::PointCloud<PointT>::Ptr transformPointCloudWithNormal(const typename pcl::PointCloud<PointT>::Ptr &cloudIn, const Eigen::Matrix4f &transform)
{
    typename pcl::PointCloud<PointT>::Ptr cloudOut(new pcl::PointCloud<PointT>());
    pcl::copyPointCloud(*cloudIn, *cloudOut);
    int cloudSize = cloudIn->size();
#pragma omp parallel for
    for (int i = 0; i < cloudSize; i++)
    {
        cloudOut->points[i].x = transform(0, 0) * cloudIn->points[i].x + transform(0, 1) * cloudIn->points[i].y + transform(0, 2) * cloudIn->points[i].z + transform(0, 3);
        cloudOut->points[i].y = transform(1, 0) * cloudIn->points[i].x + transform(1, 1) * cloudIn->points[i].y + transform(1, 2) * cloudIn->points[i].z + transform(1, 3);
        cloudOut->points[i].z = transform(2, 0) * cloudIn->points[i].x + transform(2, 1) * cloudIn->points[i].y + transform(2, 2) * cloudIn->points[i].z + transform(2, 3);
        cloudOut->points[i].normal_x = transform(0, 0) * cloudIn->points[i].normal_x + transform(0, 1) * cloudIn->points[i].normal_y + transform(0, 2) * cloudIn->points[i].normal_z;
        cloudOut->points[i].normal_y = transform(1, 0) * cloudIn->points[i].normal_x + transform(1, 1) * cloudIn->points[i].normal_y + transform(1, 2) * cloudIn->points[i].normal_z;
        cloudOut->points[i].normal_z = transform(2, 0) * cloudIn->points[i].normal_x + transform(2, 1) * cloudIn->points[i].normal_y + transform(2, 2) * cloudIn->points[i].normal_z;
    }
    return cloudOut;
}

template <typename PointT>
void drawClouds(const pcl::visualization::PCLVisualizer::Ptr &viewer, const std::vector<typename pcl::PointCloud<PointT>::Ptr> &clouds, int viewport = 0, bool showNormals = false)
{
    static std::random_device rd;
    static std::default_random_engine random(rd());
    static std::vector<std::tuple<uint8_t, uint8_t, uint8_t>> colors;

    for (size_t i = 0; i < clouds.size(); i++)
    {
        if (i >= colors.size())
        {
            uint8_t r = 50 + random() % 150;
            uint8_t g = 50 + random() % 150;
            uint8_t b = 50 + random() % 150;
            colors.emplace_back(r, g, b);
        }
        std::string id = "viewport" + std::to_string(viewport) + " :cloud" + std::to_string(i);
        pcl::visualization::PointCloudColorHandlerCustom<PointT> cloudColor(clouds[i], std::get<0>(colors[i]), std::get<1>(colors[i]), std::get<2>(colors[i]));
        if (viewer->contains(id))
        {
            viewer->updatePointCloud(clouds[i], cloudColor, id);
        }
        else
        {
            viewer->addPointCloud<PointT>(clouds[i], cloudColor, id, viewport);
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, id);
        }
        // viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, id, viewport);
        if (showNormals)
        {
            std::string normalId(id + "normals");
            viewer->removeShape(normalId);
            viewer->addPointCloudNormals<PointT>(clouds[i], 100, 5.0, normalId, viewport);

            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 1.0, normalId, viewport);
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, normalId, viewport);
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, normalId, viewport);
        }
    }
}

std::vector<std::string> GetFilesInDir(const std::string &dir, const std::string &extension)
{
    if (!std::filesystem::exists(dir))
    {
        return {};
    }
    std::vector<std::string> files;
    std::filesystem::path dirPath(dir);
    std::filesystem::directory_iterator dirItr(dirPath);
    for (const auto &entry : dirItr)
    {
        auto fsPath = entry.path();
        if (fsPath.extension() == extension)
        {
            files.emplace_back(fsPath.string());
        }
    }
    std::sort(files.begin(), files.end());
    return files;
}